using DistributedGISA, Plots

# centralized constraint
Xc = [@interval(-5,5), @interval(-5,5), @interval(-5,5)]
# number of discretization per dimension
d = 64
# discretize state constraint
dsc = DiscreteSpace(Xc,[d,d,d])
# initial approximation of CIS
Rkall = [[i,j] for i in 1:d for j in 1:d]

# subsystem 1 sets
X1 = [@interval(-5,5), @interval(-5,5)]
ds1 = DiscreteSpace(X1,[d,d])
f1(x,u) = (2.0*x[1] + u[1], x[1] + 2.0*x[2])
u = [@interval(-1,1)]
R1k = [[i,j] for i in 1:d for j in 1:d]

# subsystem 2 sets
X2 = [@interval(-5,5), @interval(-5,5)]
ds2 = DiscreteSpace(X2,[d,d])
f2(x,xi,u) = (xi[1] + 2.0*x[1], x[1] + 2.0*x[2])
R2k = [[i,j] for i in 1:d for j in 1:d]

# subsystem 1 computation
G1 = create_graph(R1k, ds1, u, f1)
nlc1 = select_nonleaving_cells(G1)
lc1 = setdiff(collect(1:length(R1k)))
R1k = R1k[nlc1]
R1 = create_boxes(R1k,ds1)

writedlm("R1.txt",unique!(vcat([collect(vertices(b)) for b in R1]...)))
# lc1 = [i for (i,x) in enumerate(outdegree(G1)) if x<=50]
R1tt = select_test_cells(nlc1,lc1,G1,d,Rkall) 

# subsystem 2 computation
G2 = create_graph(R2k, ds2, R1k, ds1, u, f2)
nlc2 = select_nonleaving_cells(G2)
lc2 = setdiff(collect(1:length(R2k)))
R2k = R2k[nlc2]
R2 = create_boxes(R2k,ds2)

writedlm("R2.txt",unique!(vcat([collect(vertices(b)) for b in R2]...)))
# lc2 = [i for (i,x) in enumerate(outdegree(G2)) if x<=50]
R2tt = select_test_cells(nlc2,lc2,G2,d,Rkall)

# extract R3k
R3k = [[i,k] for (i,j1) in R1k for (j2,k) in R2k if j1==j2]
R3 = create_boxes(R3k,ds2)

writedlm("R3.txt",unique!(vcat([collect(vertices(b)) for b in R3]...)))
# select test cells
# Rtt = select_test_cells(nlc2,G2,d,Rkall)

# reconstruct sets
# Rr,Rt = reconstruct_set(R1k, R2k, Rtt)
Rr = unique!([[i,j1,k] for (i,j1) in R1k for (j2,k) in R2k if j1==j2])

Rt = unique!([[i,j,k] for (i,j,k) in Rr if [i,j] in R1tt && [j,k] in R2tt])

# validate
f(x,u) = (2.0*x[1] + u[1], x[1] + 2.0*x[2], x[2] + 2.0*x[3])
Rkr = validate_set(Rr, Rt, dsc, u, f)

# save data
bb = create_boxes(Rkr,dsc)
writedlm("R.txt",unique!(vcat([collect(vertices(b)) for b in bb]...)))

# 


# end
println("Done!!!")

# arr = ConvexHullArray(bxs)
# ak1 = Projection(arr,[1,2])
# ak2 = Projection(arr,[2,3])

# for plotting
# R1c = 